from .arm_mpc import MPCSolver
from ezgl import Matrix4x4

class MPCMotion:
    def __init__(self, client):
        self.solver = MPCSolver(5)
        self.target_pose = None
        self.client = client

    def get_joint(self):
        current_q = self.client.get_joint()
        if self.target_pose is not None:
            target_q = self.solver.solve(current_q, self.target_pose.xyz, self.target_pose.matrix33)
        else:
             target_q = current_q
        return target_q

    def add_pose(self, target_pose: Matrix4x4):
        self.target_pose = target_pose
    
